#include "motor.h"



void Motor_Init(MOTOR_T *motor,uint8_t ID,float gear_ration,float accuracy,float wheel_diameter,TIM_HandleTypeDef *htim)
{
	motor->parameter.gear_ration = gear_ration;
	motor->parameter.accuracy = accuracy;
	motor->parameter.tyre_diameter = wheel_diameter;
	motor->parameter.ID = ID;
	motor->parameter.encoder_tim	=	*htim;
	
}

void Motor_Status_Update(MOTOR_T *motor)
{
	float delta_cnt;

	motor->status.encoder_cnt = __HAL_TIM_GET_COUNTER(&motor->parameter.encoder_tim);

	delta_cnt = motor->status.last_encoder_cnt - motor->status.encoder_cnt;
	if (delta_cnt > HalfAccuracy)
		delta_cnt -= Accuracy;
	else if (delta_cnt < -HalfAccuracy)
		delta_cnt += Accuracy;
	motor->status.encoder_speed = delta_cnt / motor->parameter.accuracy / 0.001f;

	motor->status.wheel_circumference = PI * motor->parameter.tyre_diameter;

	motor->status.actual_speed = motor->status.encoder_speed * motor->status.wheel_circumference * motor->parameter.gear_ration;

	motor->status.last_encoder_cnt = motor->status.encoder_cnt;
}
